/*
 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.box2dflash.dynamics.joints {
	import org.box2dflash.common.math.*;
	import org.box2dflash.common.*;
	import org.box2dflash.dynamics.*;
	use namespace Internal;

	/// A gear joint is used to connect two joints together. Either joint
	/// can be a revolute or prismatic joint. You specify a gear ratio
	/// to bind the motions together:
	/// coordinate1 + ratio * coordinate2 = constant
	/// The ratio can be negative or positive. If one joint is a revolute joint
	/// and the other joint is a prismatic joint, then the ratio will have units
	/// of length or units of 1/length.
	/// @warning The revolute and prismatic joints must be attached to
	/// fixed bodies (which must be body1 on those joints).
	public class GearJoint extends Joint {
		public override function get anchor1():Vec2 {
			//return m_body1->worldPoint(m_localAnchor1);
			return m_body1.worldPoint(m_localAnchor1);
		}

		public override function get anchor2():Vec2 {
			//return m_body2->worldPoint(m_localAnchor2);
			return m_body2.worldPoint(m_localAnchor2);
		}

		public override function get reactionForce():Vec2 {
			// TODO_ERIN not tested
			var F:Vec2 = new Vec2(m_force * m_J.linear2.x, m_force * m_J.linear2.y);
			return F;
		}

		public override function get reactionTorque():Number {
			// TODO_ERIN not tested
			//Vec2 r = Mul(m_body2->m_xf.R, m_localAnchor2 - m_body2->GetLocalCenter());
			var tMat:Mat22 = m_body2.m_xf.R;
			var rX:Number = m_localAnchor1.x - m_body2.m_sweep.localCenter.x;
			var rY:Number = m_localAnchor1.y - m_body2.m_sweep.localCenter.y;
			var tX:Number = tMat.col1.x * rX + tMat.col2.x * rY;
			rY = tMat.col1.y * rX + tMat.col2.y * rY;
			rX = tX;
			//Vec2 F = m_force * m_J.linear2;
			//float32 T = m_force * m_J.angular2 - Cross(r, F);
			tX = m_force * m_J.angular2 - (rX * (m_force * m_J.linear2.y) - rY * (m_force * m_J.linear2.x));
			return tX;
		}

		public function get ratio():Number {
			return m_ratio;
		}

		//--------------- Internals Below -------------------
		public function GearJoint(def:GearJointDef) {
			// parent constructor
			super(def);
		
			var type1:int = def.joint1.m_type;
			var type2:int = def.joint2.m_type;
		
			//Settings.Assert(type1 == Joint.e_revoluteJoint || type1 == Joint.e_prismaticJoint);
			//Settings.Assert(type2 == Joint.e_revoluteJoint || type2 == Joint.e_prismaticJoint);
			//Settings.Assert(def.joint1.m_body1.isStatic);
			//Settings.Assert(def.joint2.m_body1.isStatic);
			m_revolute1 = null;
			m_prismatic1 = null;
			m_revolute2 = null;
			m_prismatic2 = null;
		
			var coordinate1:Number;
			var coordinate2:Number;
		
			m_ground1 = def.joint1.m_body1;
			m_body1 = def.joint1.m_body2;
			if (type1 == Joint.e_revoluteJoint) {
				m_revolute1 = def.joint1 as RevoluteJoint;
				m_groundAnchor1.v(m_revolute1.m_localAnchor1);
				m_localAnchor1.v(m_revolute1.m_localAnchor2);
				coordinate1 = m_revolute1.jointAngle;
			}
		else {
				m_prismatic1 = def.joint1 as PrismaticJoint;
				m_groundAnchor1.v(m_prismatic1.m_localAnchor1);
				m_localAnchor1.v(m_prismatic1.m_localAnchor2);
				coordinate1 = m_prismatic1.jointTranslation;
			}
		
			m_ground2 = def.joint2.m_body1;
			m_body2 = def.joint2.m_body2;
			if (type2 == Joint.e_revoluteJoint) {
				m_revolute2 = def.joint2 as RevoluteJoint;
				m_groundAnchor2.v(m_revolute2.m_localAnchor1);
				m_localAnchor2.v(m_revolute2.m_localAnchor2);
				coordinate2 = m_revolute2.jointAngle;
			}
		else {
				m_prismatic2 = def.joint2 as PrismaticJoint;
				m_groundAnchor2.v(m_prismatic2.m_localAnchor1);
				m_localAnchor2.v(m_prismatic2.m_localAnchor2);
				coordinate2 = m_prismatic2.jointTranslation;
			}
		
			m_ratio = def.ratio;
		
			m_constant = coordinate1 + m_ratio * coordinate2;
		
			m_force = 0.0;
		}

		Internal override function initVelocityConstraints(step:TimeStep):void {
			var g1:Body = m_ground1;
			var g2:Body = m_ground2;
			var b1:Body = m_body1;
			var b2:Body = m_body2;
		
			// temp vars
			var ugX:Number;
			var ugY:Number;
			var rX:Number;
			var rY:Number;
			var tMat:Mat22;
			var tVec:Vec2;
			var crug:Number;
			var tX:Number;
		
			var K:Number = 0.0;
			m_J.zero();
		
			if (m_revolute1) {
				m_J.angular1 = -1.0;
				K += b1.m_invI;
			}
		else {
				//Vec2 ug = MulMV(g1->m_xf.R, m_prismatic1->m_localXAxis1);
				tMat = g1.m_xf.R;
				tVec = m_prismatic1.m_localXAxis1;
				ugX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
				ugY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
				//Vec2 r = Mul(b1->m_xf.R, m_localAnchor1 - b1->GetLocalCenter());
				tMat = b1.m_xf.R;
				rX = m_localAnchor1.x - b1.m_sweep.localCenter.x;
				rY = m_localAnchor1.y - b1.m_sweep.localCenter.y;
				tX = tMat.col1.x * rX + tMat.col2.x * rY;
				rY = tMat.col1.y * rX + tMat.col2.y * rY;
				rX = tX;
			
				//var crug:Number = Cross(r, ug);
				crug = rX * ugY - rY * ugX;
				//m_J.linear1 = -ug;
				m_J.linear1 = new Vec2(-ugX, -ugY);
				m_J.angular1 = -crug;
				K += b1.m_invMass + b1.m_invI * crug * crug;
			}
		
			if (m_revolute2) {
				m_J.angular2 = -m_ratio;
				K += m_ratio * m_ratio * b2.m_invI;
			}
		else {
				//Vec2 ug = Mul(g2->m_xf.R, m_prismatic2->m_localXAxis1);
				tMat = g2.m_xf.R;
				tVec = m_prismatic2.m_localXAxis1;
				ugX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
				ugY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
				//Vec2 r = Mul(->m_xf.R, m_localAnchor2 - ->GetLocalCenter());
				tMat = b2.m_xf.R;
				rX = m_localAnchor2.x - b2.m_sweep.localCenter.x;
				rY = m_localAnchor2.y - b2.m_sweep.localCenter.y;
				tX = tMat.col1.x * rX + tMat.col2.x * rY;
				rY = tMat.col1.y * rX + tMat.col2.y * rY;
				rX = tX;
			
				//float32 crug = Cross(r, ug);
				crug = rX * ugY - rY * ugX;
				//m_J.linear2 = -m_ratio * ug;
				m_J.linear2 = new Vec2(-m_ratio * ugX, -m_ratio * ugY);
				m_J.angular2 = -m_ratio * crug;
				K += m_ratio * m_ratio * (b2.m_invMass + b2.m_invI * crug * crug);
			}
		
			// Compute effective mass.
			//Settings.Assert(K > 0.0);
			m_mass = 1.0 / K;
		
			if (step.warmStarting) {
				// Warm starting.
				var P:Number = step.dt * m_force;
				//b1.m_linearVelocity += b1.m_invMass * P * m_J.linear1;
				b1.m_linearVelocity.x += b1.m_invMass * P * m_J.linear1.x;
				b1.m_linearVelocity.y += b1.m_invMass * P * m_J.linear1.y;
				b1.m_angularVelocity += b1.m_invI * P * m_J.angular1;
				//.m_linearVelocity += b2.m_invMass * P * m_J.linear2;
				b2.m_linearVelocity.x += b2.m_invMass * P * m_J.linear2.x;
				b2.m_linearVelocity.y += b2.m_invMass * P * m_J.linear2.y;
				b2.m_angularVelocity += b2.m_invI * P * m_J.angular2;
			}
		else {
				m_force = 0.0;
			}
		}

		Internal override function solveVelocityConstraints(step:TimeStep):void {
			var b1:Body = m_body1;
			var b2:Body = m_body2;
		
			var Cdot:Number = m_J.compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);
		
			var force:Number = -step.inv_dt * m_mass * Cdot;
			m_force += force;
		
			var P:Number = step.dt * force;
			b1.m_linearVelocity.x += b1.m_invMass * P * m_J.linear1.x;
			b1.m_linearVelocity.y += b1.m_invMass * P * m_J.linear1.y;
			b1.m_angularVelocity += b1.m_invI * P * m_J.angular1;
			b2.m_linearVelocity.x += b2.m_invMass * P * m_J.linear2.x;
			b2.m_linearVelocity.y += b2.m_invMass * P * m_J.linear2.y;
			b2.m_angularVelocity += b2.m_invI * P * m_J.angular2;
		}

		Internal override function solvePositionConstraints():Boolean {
			var linearError:Number = 0.0;
		
			var b1:Body = m_body1;
			var b2:Body = m_body2;
		
			var coordinate1:Number;
			var coordinate2:Number;
			if (m_revolute1) {
				coordinate1 = m_revolute1.jointAngle;
			}
		else {
				coordinate1 = m_prismatic1.jointTranslation;
			}
		
			if (m_revolute2) {
				coordinate2 = m_revolute2.jointAngle;
			}
		else {
				coordinate2 = m_prismatic2.jointTranslation;
			}
		
			var C:Number = m_constant - (coordinate1 + m_ratio * coordinate2);
		
			var impulse:Number = -m_mass * C;
		
			b1.m_sweep.c.x += b1.m_invMass * impulse * m_J.linear1.x;
			b1.m_sweep.c.y += b1.m_invMass * impulse * m_J.linear1.y;
			b1.m_sweep.a += b1.m_invI * impulse * m_J.angular1;
			b2.m_sweep.c.x += b2.m_invMass * impulse * m_J.linear2.x;
			b2.m_sweep.c.y += b2.m_invMass * impulse * m_J.linear2.y;
			b2.m_sweep.a += b2.m_invI * impulse * m_J.angular2;
		
			b1.synchronizeTransform();
			b2.synchronizeTransform();
		
			return linearError < Settings._linearSlop;
		}

		public var m_ground1:Body;
		public var m_ground2:Body;
		// One of these is NULL.
		public var m_revolute1:RevoluteJoint;
		public var m_prismatic1:PrismaticJoint;
		// One of these is NULL.
		public var m_revolute2:RevoluteJoint;
		public var m_prismatic2:PrismaticJoint;
		public var m_groundAnchor1:Vec2 = new Vec2();
		public var m_groundAnchor2:Vec2 = new Vec2();
		public var m_localAnchor1:Vec2 = new Vec2();
		public var m_localAnchor2:Vec2 = new Vec2();
		public var m_J:Jacobian = new Jacobian();
		public var m_constant:Number;
		public var m_ratio:Number;
		// Effective mass
		public var m_mass:Number;
		// Impulse for accumulation/warm starting.
		public var m_force:Number;
	}
}
